#include "pcl_module.h"
#include <pcl/surface/gp3.h>
#include <pcl/point_cloud.h >
#include <pcl/io/pcd_io.h>

#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/segmentation/region_growing.h>

#include <pcl/filters/random_sample.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/correspondence_rejection_surface_normal.h>
#include <pcl/features/fpfh_omp.h >

QVector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> PCLModule::m_targetCloud;
QVector<QVector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>> PCLModule::m_measureClouds;

pcl::PointCloud<pcl::PointXYZRGB>::Ptr PCLModule::CoarseRegist(int index)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr globalCloud = m_measureClouds.at(index).at(0);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr globaL_targetcloud = m_targetCloud.at(index);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_source(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_DownSampling_source(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_DownSampling_target(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::VoxelGrid<pcl::PointXYZRGB> sor;
    int leaf_size = 2;//体素大小
    sor.setLeafSize(leaf_size, leaf_size, leaf_size);
    sor.setInputCloud(globalCloud);
    sor.filter(*cloud_DownSampling_source);
    sor.setInputCloud(globaL_targetcloud);
    sor.filter(*cloud_DownSampling_target);
    cloud_source = cloud_DownSampling_source;
    cloud_target = cloud_DownSampling_target;

    pcl::PointCloud<pcl::Normal>::Ptr normals_source(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimator_source;
    normal_estimator_source.setInputCloud(cloud_source);
    normal_estimator_source.setRadiusSearch(5);
    normal_estimator_source.compute(*normals_source);

    pcl::FPFHEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33> fpfhs_source;
    fpfhs_source.setInputCloud(cloud_source);
    fpfhs_source.setInputNormals(normals_source);
    fpfhs_source.setNumberOfThreads(4);
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
    fpfhs_source.setSearchMethod(tree);
    fpfhs_source.setRadiusSearch(10);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_s(new pcl::PointCloud<pcl::FPFHSignature33>());
    fpfhs_source.compute(*fpfhs_s);

    pcl::PointCloud<pcl::Normal>::Ptr normals_target(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimator_target;
    normal_estimator_target.setInputCloud(cloud_target);
    normal_estimator_target.setRadiusSearch(5);
    normal_estimator_target.compute(*normals_target);

    pcl::FPFHEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33> fpfhs_target;
    fpfhs_target.setInputCloud(cloud_target);
    fpfhs_target.setInputNormals(normals_target);
    fpfhs_target.setNumberOfThreads(4);
    fpfhs_target.setSearchMethod(tree);
    fpfhs_target.setRadiusSearch(10);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_t(new pcl::PointCloud<pcl::FPFHSignature33>());
    fpfhs_target.compute(*fpfhs_t);

    pcl::CorrespondencesPtr correspondences(new pcl::Correspondences);
    pcl::registration::CorrespondenceEstimation<pcl::FPFHSignature33, pcl::FPFHSignature33> corr_estimation;
    corr_estimation.setInputSource(fpfhs_s);
    corr_estimation.setInputTarget(fpfhs_t);
    corr_estimation.determineCorrespondences(*correspondences);

    pcl::registration::CorrespondenceRejectorSurfaceNormal::Ptr rejector_surface_normal(new pcl::registration::CorrespondenceRejectorSurfaceNormal);
    rejector_surface_normal->setInputSource<pcl::PointXYZRGB>(cloud_source);
    rejector_surface_normal->setInputTarget<pcl::PointXYZRGB>(cloud_target);

    rejector_surface_normal->setThreshold(0.5);
    rejector_surface_normal->getRemainingCorrespondences(*correspondences, *correspondences);

    pcl::registration::TransformationEstimationSVD<pcl::PointXYZRGB, pcl::PointXYZRGB> trans_est;
    Eigen::Matrix4f transform_coarse_registration1;
    trans_est.estimateRigidTransformation(*cloud_source, *cloud_target, *correspondences, transform_coarse_registration1);
    pcl::transformPointCloud(*cloud_source, *cloud_source, transform_coarse_registration1);
    pcl::transformPointCloud(*globalCloud, *globalCloud, transform_coarse_registration1);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_source3(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_target3(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_DownSampling_source3(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_DownSampling_target3(new pcl::PointCloud<pcl::PointXYZRGB>);

    sor.setInputCloud(globalCloud);
    sor.filter(*cloud_DownSampling_source3);
    sor.setInputCloud(globaL_targetcloud);
    sor.filter(*cloud_DownSampling_target3);
    cloud_source3 = cloud_DownSampling_source3;
    cloud_target3 = cloud_DownSampling_target3;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

    pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
    icp.setInputSource(cloud_source3);
    icp.setInputTarget(cloud_target3);

    icp.setEuclideanFitnessEpsilon(0.001);
    icp.setTransformationEpsilon(0.001);
    icp.setMaximumIterations(1000);

    icp.align(*aligned_cloud);
    Eigen::Matrix4f transformation3 = icp.getFinalTransformation();
    pcl::transformPointCloud(*globalCloud, *globalCloud, transformation3);
    return globalCloud;
}
